% v10: 
% v20: using Nearest Neighbor Matching and RANSAC -> coding structure
% stucks here when the spinal structure is greatly modified!!
% v30: changing the whole critical structure
% v32: neighbor region filter to enhance minDistSearch
% v40: use 4 points homography
% v50: change model data structure to a set of segments
% v55: work with enhanced edge neighbor
% v57: work with robust edge neighbor
function [model_O, model_para_O] = RSpatternTrackerRUN( model,model_para,pts,r1,c1,iterNum,thDist,thInlrRatio ) 

    sampleNum = 5;
 
    ptNum = size(pts,2)
    thInlr = round(thInlrRatio*ptNum);
    inlrNum = zeros(1,iterNum);
    Hrec = zeros(3,3,iterNum);
    dist1=zeros(1,iterNum);
    movThres = 40; %10; %100; % 'small move' threshold
    PassThres = 100;
    cntPthres = 0;
    
    % debug var
    cntsmallmov = 0;
    cntprop = 0;
    cnt_nb0 = 0;
%     cnt_nb1 = 0;
    
    % Get current pose
    v = model(:,:,1); v1 = model(:,:,2); v2 = model(:,:,3); %v3 = model{2}(:,:,2);
    A = v(:,1);
    B = v(:,2);
    C = v1(:,2);
    D = v2(:,1);
    %E = v3(size(v3,1),:);
%     pose = [A' B' C' D' E']
    pose = [A B C D];
    
    % Set model neighbor 
    minx_nb = min(pose(1,:));
    miny_nb = min(pose(2,:));
    maxx_nb = max(pose(1,:));
    maxy_nb = max(pose(2,:));
    nbThr = 20;
    minx_nb1 = minx_nb-nbThr;
    maxx_nb1 = maxx_nb+nbThr;
    miny_nb1 = miny_nb-nbThr;
    maxy_nb1 = maxy_nb+nbThr;
    
    % Choose samples
    num = 2;
    Neibor = NearestCorners(pose,[c1';r1'],num);
    samples = [c1';r1'];
    flg = zeros(size(c1,1));
    
    p = 0;
%     for i=1:num,
%         for j=1:num,
%             for k=1:num,
%                 for l=1:num,
%                     for m=1:num,
    
    num_nb = 2;  % 2 is too smal to be stable
    for i=1:num_nb,
        for j=1:num_nb,
            for k=1:num_nb,
                for l=1:num_nb,
%                     for m=1:2,
                       
                        p = p + 1;
                        ptSample = [];
                        % Extracting sample set
                        ptSample(:,1) = samples(:,Neibor(i,1));
                        flg(Neibor(i,1)) = 1;
                        
                        if ~flg(Neibor(j,2))
                            ptSample(:,2) = samples(:,Neibor(j,2));
                            flg(Neibor(j,2)) = 1;
                        else 
                            continue;
                        end
                        
                        if ~flg(Neibor(k,3))
                            ptSample(:,3) = samples(:,Neibor(k,3));
                            flg(Neibor(k,3)) = 1;
                        else 
                            continue;
                        end
                        
                        if ~flg(Neibor(l,4))
                            ptSample(:,4) = samples(:,Neibor(l,4));
                            flg(Neibor(l,4)) = 1;
                        else 
                            continue;
                        end
                        
%                         if ~flg(Neibor(m,5))
%                             ptSample(:,5) = samples(:,Neibor(m,5));
%                             flg(Neibor(m,5)) = 1;
%                         else 
%                             continue;
%                         end
                        
                        flg = zeros(size(c1,1));
                        
                        % %%%%%% CHECK & RANSAC THE SAMPLE HERE
                        % Check if this is 'proper' sample set
%                         if ~checkSample(ptSample),
%                             cntprop = cntprop + 1;
%                             continue;
%                         end;
                        
                        % Check 'small movement' cond. in 'next' mode
%                         if norm(ptSample-pose) > movThres, 
%                             cntsmallmov = cntsmallmov + 1; 
%                             continue; 
%                         end;
                        % %%%%%%
                      
                        % Calculate homography model H
%                         pose;
%                         ptSample;
                        H = HomoMtrxCalc(pose, ptSample); % BUG here
                        
                        % Reproject model with H
                        modeli = homography(model, H);
                        modeli_para = XtrackMDLpara(modeli);
                        %if (iscell(modeli)==0), input('No Projection!!'); continue; end;
                        
                        % 2. count the inliers, if more than thInlr, refit; else iterate
%                         cnt_nb0 = 0;
                        
                        for ii=1:ptNum
                            % model neigbor criteria
                            if (pts(1,ii)>=minx_nb1)&&(pts(1,ii)<=maxx_nb1)&& ...
                                (pts(2,ii)>=miny_nb1)&&(pts(2,ii)<=maxy_nb1)
                            
                                val = minDistSearch(pts(:,ii),modeli,modeli_para);
                                dist1(ii) = val;
%                                 if isinf(val)
%                                     cnt_nb1 = cnt_nb1 + 1; 
%                                 end
                            else
                                dist1(ii) = inf;
%                                 cnt_nb0 = cnt_nb0 + 1;
                            end
                        end
                        
%                         cnt_nb0 
%                         cnt_nb1
                        
                        inlier1 = find(abs(dist1) < thDist);
                        inlrNum(p) = length(inlier1);
                        %     if length(inlier1) < thInlr, continue;  end
                        %     if strcmp(mode,'next'),  input('run through already');end
                        Hrec(:,:,p)=H;
%                         ptSampleR(:,:,p)=ptSample;
                        %     cntPthres = cntPthres + 1;
                        %     if cntPthres >= PassThres,
                        %         break;
                        %     end
                        
%                     end
                end
            end
        end
    end
            
% 3. choose the coef with the MOST INLINERS (not the minimum distance)
[q,idx] = max(inlrNum)% 
ret=Hrec(:,:,idx); 

modeli = homography(model, ret);
modeli_para = XtrackMDLpara(modeli);

% plotmodel(modeli);

  % model = RSpatternTracker(pts,iterNum,thDist,thInlrRatio);
    
    % Output estimation result
    % plotmodel(model);
% cntsmallmov
% cntprop
% runcase = num^4 % num^5
% pc_mov = cntsmallmov/iterNum*100  % pc = percent
% pc_cntprop = cntprop/iterNum*100

model_O = modeli;
model_para_O  = modeli_para;

end


